COMMENT ⊗   VALID 00008 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00002 00002	  Data structures, GSINIT
C00007 00003	  NXTTIM
C00008 00004	  INVLDT, INVLR0
C00010 00005	  CHANGE
C00012 00006	  GETVAL
C00014 00007	  EVAL, GETDEV
C00020 00008	  MFRAME, KFRAME
C00025 ENDMK
C⊗;
;  Data structures, GSINIT

.SBTTL Graph routines.
;Graph structure definitions
;RHT 9/74  RF 6/75, 10/75   totally redone ARG 4/78

COMMENT ⊗  
This is the runtime's prime evil,
The murderous graph nodes and interlocks.
⊗

;GRAPH NODES

;VARIABLE NODES		;Explicitly released, formed from large block store.
	II==0
	XX  NEXT	;Links all graph nodes.  Points to next one.
	XX  TYPE	;Mode bits
	 FTYPE == 1	;Set if variable node, zero for device node
	 DYNAM == 2	;Set if variable is affixed to device (only arms)
	XX  INVMRK	;0 => valid, other => invalid
	XX  VAL		;points at the value cell
	XX  CALCS	;list of calculators - currently only affixments
	XX  DCNT	;counter used for affixment to dynamic device
			; high byte gives device: yarm or barm, low byte gives cntr
	VNDSIZ == II/2	;Length of variable node (in words)

;DEVICE NODES		;Explicitly released, formed from large block store.
	II==0
	XX  NEXT	;Not used for devices
	XX  TYPE	;Mode bits.  0: device, 1: variable
	 SCDEV == 400	;Set if device has a scalar value
	XX  COLST	;Coefficient list for WHERE in ARM.PAL
	XX  COLST2	;Second word of coefficient list
	XX  CALCS	;list of calculators - currently only affixments
	XX  MECH	;Mechanism bits for UPDATE in ARM.PAL

;AFFIXMENT NODE		;Explicitly released, formed from large block store.
	II==0
	XX  NEXT	;next calculator cell in chain
	XX  TYPE	;Type bits specifying what sort of affixment/calculator
	 AFXTYP == 1	;Set if affixment calculator 
	 NONRGD == 400  ;Set if non-rigid affixment
	 FRAME2 == 1000	;Set if second frame in affixment
	 EXPTRN == 2000	;Set if an explicit trans is used
	XX  OTHER	;pointer to other frame this one is affixed to
	XX  TRANS	;pointer to trans used for this affixment
	XX  NEXT2	;next calculator cell in chain for frame2
	XX  TYPE2	;Type bits for other (same as above - FRAME2 always set)
	XX  US		;pointer to us from frame2
	AFXSIZ == II/2	;Size of affixment cell, in words

;SPECIAL DEVICE CALCULATOR NODE
	II==0
	XX  NEXT	;next calculator cell in chain
	XX  TYPE	;Type bits specifying what sort of affixment/calculator
	XX  CLCRTN	;Address of calculator routine
	XX  CHNRTN	;Address of changer routine


DATA
GNODES:  .BLKW 1	;head of chain of graph nodes.
TIME:	0		;used during evaluation of nodes
GNEVT:	.BLKW 1		;event for interlocking graph references
CODE

GSINIT:
;Initialize the graph structure to a null situation;
	EVMAK	;Make a new interlock event.
	MOV (SP),GNEVT;
	EVSIG 	;Give it one signal.
	CLR GNODES;
	CLR TIME;
	RTS PC	;Done

;  NXTTIM

COMMENT ⊗
	JSR	PC,NXTTIM
 	
Returns TIME←TIME+1 in R0.  If TIME goes negative then set all
positive mark cells to negative, then set time to 1. ⊗


NXTTIM:	INC	TIME		;TIME←TIME+1
	MOV	TIME,R0
	BGT	4$		;OK?
	MOV	GNODES,R0	;
	BEQ	3$		;DID WE HAVE ANY??
1$:	TST	INVMRK(R0)	;YES
	BLE	2$		;WAS INVMRK POSITIVE
	NEG	INVMRK(R0)	;YES, NEGATE IT
2$:	MOV	(R0),R0		;GO ON TO NEXT
	BNE	1$		;IF ANY
3$:	INC	R0		;R0←0+1
	MOV	R0,TIME		;TIME IS 1 AGAIN
4$:	RTS	PC

;  INVLDT, INVLR0

INVLDT:	
COMMENT ⊗ Called only from the outside world, only for devices after a MOVE.
R0 is the node to invalidate, along with all dependents.  We must invalidate
dependents even if the given node is already invalid, unless we have just now
invalidated it, which would imply that we are in a cycle.  ⊗
	EVWAIT	GNEVT		;We change TIME, so must lock this
	MOV	R0,R1
	JSR	PC,NXTTIM
	MOV	R1,R0
	JSR	PC,INVLR0
	EVSIG	GNEVT		;End of critical section
	RTS	PC

INVLR0:	BIT	#FTYPE,TYPE(R0)	;See if it's a variable or device node
	BNE	1$
	CMP	COLST2(R0),TIME	;Device - Are we in a cycle?
	BEQ	7$		;Yes.  Return.
	MOV	TIME,COLST2(R0)	;No.  Invalidate this node.
	BR	2$		;Go invalidate the devices calcs
1$:	CMP	INVMRK(R0),TIME	;Are we in a cycle?
	BEQ	7$		;Yes.  Return.
	MOV	TIME,INVMRK(R0)	;No.  Invalidate this node.
2$:	MOV	R2,-(SP)	;Save R2 for recursive call
	MOV	CALCS(R0),R2	;R2 ← list of calculators
	BEQ	6$		;If any 
3$:	BIT	#FRAME2,TYPE(R0)	;Must be rigid or frame2
	BNE	4$
	BIT	#NONRGD,TYPE(R0)
	BNE	5$
4$:	MOV	OTHER(R2),R0	;R0 ← frame affixed to this one
	JSR	PC,INVLR0	;Go Invalidate it.
5$:	MOV	(R2),R2		;Repeat for the rest
	BNE	3$		;If any
6$:	MOV	(SP)+,R2	;Restore R2
7$:	RTS	PC

;  CHANGE

COMMENT ⊗ Called by the outside world to put a new value, CHG.VAL,
in the variable node CHG.ND. The transes for non-rigid affixments are
updated if required. ⊗

ROUTINE CHANGE,<CHG.ND,CHG.VAL>
	EVWAIT	GNEVT		;enter critical region
	JSR PC,NXTTIM		;TIME ← TIME + 1
	MOV CHG.ND(RF),R0	;R0 ← the target node
	JSR PC,INVLR0		;Invalidate it for now, along with its dependents
	MOV CHG.ND(RF),R0
	MOV CHG.VAL(RF),VAL(R0)	;Store the new value away
	CLR INVMRK(R0)		;Mark us as valid
	EVSIG GNEVT		;leave critical region

	MOV CALCS(R0),R1	;R1 ← list of calculators for frame
	BEQ 4$			;  if any
1$:	BIT #NONRGD,TYPE(R1)	;Must be non-rigid affixment
	BEQ 3$
	BIT #FRAME2,TYPE(R1)	; & also first frame
	BNE 3$
	PUSH <R1>
	CALL GETVAL,<OTHER(R1)>
	MOV R0,-(R3)		;Push value of second frame
	JSR PC,TINVRT		;Invert it
	MOV CHG.VAL(RF),-(R3)	;Push new value
	JSR PC,TTMUL		;trans ← first * inv(second)
	POP <R1>
	BIT #EXPTRN,TYPE(R1)	;check whether trans is implicit or explicit
	BNE 2$
	MOV (R3)+,TRANS(R1)	;implicit
	BR 3$
2$:	MOV (R3)+,@TRANS(R1)	;explicit
3$:	MOV (R1),R1		;check rest of calcs
	BNE 1$			;  if any
4$:	RTS PC			;all done

;  GETVAL

COMMENT ⊗ Called by the outside world.  Returns LOC[value(GTV.ND)] in R0, after
having scrounged around to get a valid value, if necessary and possible.  ⊗

ROUTINE GETVAL,<GTV.ND>
	MOV GTV.ND(RF),R2	;R2 ← LOC[variable to evaluate]
	BIT #FTYPE,TYPE(R2)	;See if frame or device
	BNE 1$
	JSR PC,GETDEV		;get current value for the device
	MOV (R3)+,R0		;R0 ← LOC[value]
	BR 4$
1$:	BIT #DYNAM,TYPE(R2)	;Affixed to a dynamic device?
	BNE 2$			;Yup - need to calculate current value
	TST INVMRK(R2)		;Is the current value good?
	BEQ 3$			;Yes
2$:	EVWAIT GNEVT		;No.  Enter critical region.
	JSR PC,NXTTIM		;TIME ← TIME + 1
	PUSH <R2>
 	CALL EVAL,<R2>		;try to evaluate the variable
	EVSIG GNEVT		;Leave critical region
	POP <R2>
3$:	MOV VAL(R2),R0		;R0 ← value cell
4$:	RTS PC			;Done

;  EVAL, GETDEV

COMMENT ⊗ EVAL is a recursive procedure, which is given EVL.ND, the target
node to evaluate.  If necessary, it calls itself at the current TIME to
track down a chain of related nodes.  GNEVT exclusion should be on before
this routine is first called, and will remain on after the return. ⊗

ROUTINE EVAL,<EVL.ND>
	MOV EVL.ND(RF),R2	;R2 ← target graph node
	CMP TIME,INVMRK(R2)	;Have we already looked at it this time?
	BEQ 15$			;Yes
	MOV TIME,INVMRK(R2)	;No - mark it
	MOV CALCS(R2),R1	;R1 ← list of calculators
	BEQ 15$			;  if any
1$:	BIT #AFXTYP,TYPE(R1)	;Make sure it's a valid affixment
	BEQ 4$
	BIT #NONRGD,TYPE(R1)	;See if someone it's affixed to is now valid
	BEQ 2$
	BIT #FRAME2,TYPE(R1)	;Must be rigid or first frame
	BNE 4$
2$:	MOV OTHER(R1),R0	;R0 ← frame/device we're affixed to
	BIT #FTYPE,TYPE(R0)	;frame or device
	BNE 3$
	PUSH <R1>		;Device
	MOV R0,R2
	JSR PC,GETDEV		;Put current value on R3 stack
	POP <R1>
	BR 9$			;Go multiply the value by the trans
3$:	BIT #DYNAM,TYPE(R0)	;If dynamic frame then never valid
	BNE 4$
	TST INVMRK(R0)		;Frame - is it valid
	BEQ 8$			;  if so go use it
4$:	MOV (R1),R1		;Try next calc
	BNE 1$			;  if any more

	MOV CALCS(R2),R1	;Check through the calcs again - this time
5$:	BIT #AFXTYP,TYPE(R1)	;  trying to evaluate the other frame
	BEQ 7$
	BIT #NONRGD,TYPE(R1)
	BEQ 6$
	BIT #FRAME2,TYPE(R1)	;Must be rigid or first frame
	BNE 7$
6$:	MOV OTHER(R1),R0	;R0 ← frame we're affixed to
	PUSH <R0,R1>
	CALL EVAL,<R0>		;Try to evaluate it
	POP <R1,R0>
	TST INVMRK(R0)		;Does it now have a value?
	BEQ 8$			;Success - go use it
7$:	MOV (R1),R1		;Nope - try the next
	BNE 5$			;  if any
	BR 15$			;No more to try - give up

8$:	MOV VAL(R0),-(R3)	;Push value of other
9$:	BIT #FRAME2,TYPE(R1)	;first or second frame?
	BNE 11$
	BIT #EXPTRN,TYPE(R1)	;explicit or implicit trans?
	BNE 10$
	MOV TRANS(R1),-(R3)	;push trans
	BR 14$
10$:	MOV @TRANS(R1),-(R3)
	BR 14$
11$:	BIT #EXPTRN,TYPE(R1)	;second frame
	BNE 12$
	MOV TRANS-10(R1),-(R3)
	BR 13$
12$:	MOV @TRANS-10(R1),-(R3)
13$:	JSR PC,TINVRT		;second ← inv(trans) * first
14$:	JSR PC,TTMUL		;first ← trans * second
	MOV EVL.ND(RF),R2	;R2 ← node we want to evaluate
	MOV (R3)+,VAL(R2)	;Transfer the value
	CLR INVMRK(R2)		;Mark it as valid
15$:	RTS PC			;Done


COMMENT ⊗ GETDEV gets the current value of the device whose node is pointed to
by R2, and places it on the R3 stack. ⊗

GETDEV:	MOV #DVBKSZ,R0		;Get a device block
	JSR PC,GTFREE
	MOV R0,R1		;R1 ← LOC[device block]
	PUSH <R1,R2>
	MOV R2,R0
	ADD #COLST,R0		;R0 ← LOC[coefficient list]
	CLR COLST2(R2)		;Make sure second word of coefficient list is zero
	JSR PC,@LWHERE		;Update the joint angles
	POP <R2,R0>
	JSR PC,RLFREE		;Release the device block
	BIT #SCDEV,TYPE(R2)	;scalar or frame device?
	BEQ 1$
	JSR PC,GETSCA		;R0 ← -(R3) ← LOC[new scalar]
	BR 2$
1$:	JSR PC,GETTRN		;R0 ← -(R3) ← LOC[new trans]
2$:	MOV MECH(R2),R2		;R2 ← mechanism bits
	MOV LTHPTR,R1		;R1 ← LOC[joint angles]
	JSR PC,@LUPDATE		;Converts joint space to cartesian coords
	CMP R2,TURNSH		;Kludge to return turns in revolutions, not degs
	BNE 3$
	LDF @(R3),AC0		;Get degrees
	DIVF C360,AC0		;Convert it to revolutions
	STF AC0,@(R3)
3$:	RTS PC			;Done

DATA
C360:	.FLT2 360.0
CODE

;  MFRAME, KFRAME

COMMENT ⊗ MFRAME is a routine to create a new frame header, and is called by
th interpreter routine AFFIX with R0 pointing at the environment entry for this
frame. ⊗

MFRAME:	PUSH <R0>		;Save environment pointer
	MOV #VNDSIZ,R0		;Size of frame header
	JSR PC,GTFREE		;R0 ← LOC[new header]
	MOV #FTYPE,TYPE(R0)	;Indicate that we're a frame header
	EVWAIT GNEVT		;Begin critical section
	MOV GNODES,(R0)		;Link us into graph node list
	MOV R0,GNODES
	MOV (SP),R1		;R1 ← Environment pointer
	BIS #HDRTYP,(R1)+	;Set header access bit in environment
	MOV (R1),VAL(R0)	;Store old value - if any
	BNE 1$
	MOV #-1,INVMRK(R0)	;If no old value set invalid
1$:	MOV R0,(R1)		;Make environment point to header
	EVSIG GNEVT		;End critical section
	POP <R0>		;Restore environment pointer
	RTS PC

COMMENT ⊗ KFRAME is a routine to destroy a frame header. It is called by the
interpreter routine KVAR with (R2) pointing to the frame header. Before killing
the header we try to validate any other frames dependent on it. ⊗

KFRAME:	PUSH <R1,R2>
	MOV (R2),R1		;R1 ← LOC[frame header]
	MOV CALCS(R1),R1	;R1 ← list of calculators
	BEQ 4$			;  if any
1$:	MOV OTHER(R1),R2	;R2 ← LOC[frame header for other frame]
	BIT #FTYPE,TYPE(R2)	;Must be a frame
	BEQ 3$
	TST INVMRK(R2)		; & currently invalid
	BEQ 3$
	BIT #NONRGD,TYPE(R1)	;Must be rigid affixment or second frame
	BEQ 2$
	BIT #FRAME2,TYPE(R1)
	BEQ 3$
2$:	PUSH <R1>
	CALL GETVAL,<R2>	;Try to validate the other frame
	POP <R1>
3$:	MOV (R1),R1		;Deal with next calc
	BNE 1$			;  if any

4$:	POP <R2>		;R2 ← environment pointer for this frame
	MOV (R2),R1		;R1 ← LOC[frame header to kill]
	EVWAIT GNEVT		;Enter critical section
	MOV CALCS(R1),R1	;R1 ← list of affixments to undo
	BEQ 10$			;  if any
5$:	PUSH <(R1)>
	MOV OTHER(R1),R0	;R0 ← LOC[frame we're affixed to]
	ADD #CALCS,R0		;R0 ← LOC[1st affixment for other]
	BIT #FRAME2,TYPE(R1)
	BNE 6$
	PUSH <R1>		;Save LOC[affixment node]
	ADD #10,R1		;R1 ← LOC[node as seen by other]
	BR 7$
6$:	SUB #10,R1
	PUSH <R1>
7$:	CMP (R0),R1		;Find affixment node in other frames calc list
	BEQ 8$			;Got it!
	MOV (R0),R0		;Check next node
	BNE 7$
	BR 9$			;If not there forget it
8$:	MOV (R1),(R0)		;Unlink node from other's calc list
9$:	POP <R0>		;R0 ← LOC[affixment node]
	JSR PC,RLFREE		;Release it
	POP <R1>		;R1 ← LOC[next affixment node to clobber]
	BNE 5$			;  if any

10$:	MOV #GNODES,R1		;R1 ← head of graph node list
	MOV (R2),R0		;R0 ← LOC[frame header being killed]
11$:	CMP (R1),R0		;Find us in list
	BEQ 12$
	MOV (R1),R1		;Move down list
	BNE 11$
	BR 13$			;Whoops, we weren't there! error, but forget it
12$:	MOV (R0),(R1)		;Unlink us from list
13$:	JSR PC,RLFREE		;Deallocate frame header
	EVSIG GNEVT		;End of critical section
	POP <R1>
	RTS PC